/**
 * \file main.cc
 * \brief Generic VBE based display driver
 */

/*
   Copyright 2007 Corey Tabaka

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#include <modinit.h>
#include <mutex.h>
#include <Log.h>
#include <Thread.h>
#include <Process.h>
#include <Namespace.h>
#include <syscall.h>
#include <platform/Interrupt.h>

#include <stdio.h>
#include <string.h>

#include <vbe/vbe.h>

const char *module_name = "display";

#define __IN_KERENL_MODULE__
#include "display.h"

typedef struct {
	uint32 num;
	uint8 * mem;
	uint16 width;
	uint16 height;
	uint8 bpp;
} texinfo;

#if 1
#define OUT(msg, args...) Log::printf(LOG_LOW, msg,##args)
#elif 0
#define OUT printf
#else
#define OUT(msg, args...) //Log::printf(LOG_LOW, "Test string")
#endif

#define DATA_ARRAY_NAME vesa_module
#include "vesa.bin.h"
#undef DATA_ARRAY_NAME

/**
 * Embedded terminal font
 */
#define DATA_ARRAY_NAME terminal_font
#include "terminal_font.font.h"
#undef DATA_ARRAY_NAME
#undef DATA_ARRAY_ATTR

/**
 * Module variables
 */

namespace hw {
	namespace console {
		static Var version("hw.console.version", "1.0", VF_SYSTEM);
	}
	
	namespace display {
		namespace current {
			static Var width("hw.display.current.width", 1024, VF_SYSTEM);
			static Var height("hw.display.current.height", 768, VF_SYSTEM);
			static Var depth("hw.display.current.bpp", 32, VF_SYSTEM);
		}
		
		namespace controller {
			static Var memory("hw.display.controller.memory", 0LL, VF_SYSTEM);
			static Var name("hw.display.controller.name", "unknown", VF_SYSTEM);
			static Var vendor("hw.display.controller.vendor", "unknown", VF_SYSTEM);
		}
	}
	
	namespace monitor {
		static Var width("hw.monitor.width", 1024, VF_SYSTEM);
		static Var height("hw.monitor.height", 768, VF_SYSTEM);
		static Var name("hw.monitor.name", "unknown", VF_SYSTEM);
		static Var vendor("hw.monitor.vendor", "unknown", VF_SYSTEM);
		
		namespace edid {
			static Var version("hw.monitor.edid.version", "", VF_SYSTEM);
			static Var revision("hw.monitor.edid.revision", "", VF_SYSTEM);
		}
		
		namespace dpms {
			static Var activeoff("hw.monitor.dpms.activeoff", 0LL, VF_SYSTEM);
			static Var suspend("hw.monitor.dpms.suspend", 0LL, VF_SYSTEM);
			static Var standby("hw.monitor.dpms.standby", 0LL, VF_SYSTEM);
		}
	}
}

RefProcess vesa;
RefMessagePipe pipe;

static vbe_controller_info_t controller;
static vbe_modeinfo_t modeinfo;
static bool modeSet = false;

static uint8 *frameBuffer;
static uint32 frameBufferSize;
static uint32 frameBufferOffset;

static vbe_ddc_caps_t ddcCaps;
static vbe_edid_t edid;
static bool readEDID;

// from agp.cc
bool initAGP(void);

static bool initVesaInfo(RefMessagePipe mp);
static void parseEDID(RefMessagePipe mp);
static bool setMode(RefMessagePipe mp, uint32 width, uint32 height,
	uint8 bitsPerPixel);
static int swapBuffers(RefMessagePipe mp);
	
static void drawString(const char *str, int x, int y, uint32 fg, uint32 bg);
static void solidFill(uint32 color);
static void solidFill(uint32 color, sint16 xs, sint16 ys, uint16 w, uint16 h);
static void drawBitmap(const texinfo *tex, sint16 x, sint16 y, uint32 fg,
	uint32 bg);
static void fillBitmap(const texinfo *tex, sint16 xs, sint16 ys, uint16 w,
	uint16 h, uint32 fg, uint32 bg);

static bool syscallHandler(regs_t *regs);

void init(void *) {
	// initialize AGP
	if (initAGP()) {
		OUT("AGP initialized\n");
	} else {
		OUT("AGP not supported\n");
	}
	
	OUT("Contacting VESA service\n");
	
	// wait for the VESA service to initialize and create its pipe
	printf("Waiting for VESA service");
	while (!(pipe = vesa->getPipe(1))) {
		printf(".");
		SLEEP(100);
	}
	printf(" Ready\n");
	
	if (initVesaInfo(pipe)) {
		if (!setMode(pipe, hw::monitor::width.ivalue,
			hw::monitor::height.ivalue, 32)) {
			// try a default minimum
			if (!setMode(pipe, 1024, 768, 32)) {
				return;
			}
		}
		
		Namespace::registerVar(hw::console::version);
		Namespace::registerVar(hw::display::current::width);
		Namespace::registerVar(hw::display::current::height);
		Namespace::registerVar(hw::display::current::depth);
		Namespace::registerVar(hw::display::controller::name);
		Namespace::registerVar(hw::display::controller::vendor);
		Namespace::registerVar(hw::display::controller::memory);
		
		if (readEDID) {
			Namespace::registerVar(hw::monitor::edid::version);
			Namespace::registerVar(hw::monitor::edid::revision);
			Namespace::registerVar(hw::monitor::name);
			Namespace::registerVar(hw::monitor::vendor);
			Namespace::registerVar(hw::monitor::width);
			Namespace::registerVar(hw::monitor::height);
			Namespace::registerVar(hw::monitor::dpms::activeoff);
			Namespace::registerVar(hw::monitor::dpms::suspend);
			Namespace::registerVar(hw::monitor::dpms::standby);
		}
		
		Interrupt::registerHandler(INT_SYSCALL, syscallHandler);
		
		syscall(SYSCALL_DISPLAY, SYSCALL_DISPLAY_DRAW_STRING,
			"Experimental Kernel v0.01  Copyright (c) Corey Tabaka 2007",
			0, 0, 0xffffff, ~0);
	} else {
		OUT("Failed to initialize VESA\n");
	}
}

extern "C"
void __init module_init(void) {
	printf("Initializing display driver\n");
	
	// load the 16 bit VESA service
	vesa = Process::createProcess("VESA Process");
	vesa->getPageMap().initialize(0, (1024 + 64) * 1024);
	vesa->getAddressSpace().create(0, 0, (1024 + 64) * 1024,
		REGION_FLAGS_VM86, REGION_TYPE_VM86);
	// load the binary at 0050:0100 (00000600)
	vesa->loadBinary(vesa_module, sizeof(vesa_module), 0x600, false);
	
	RefThread thread = VM86Thread::createThread((ThreadEntryPoint) 0x100, 0,
		"VESA Thread", vesa);
	thread->run();
	
	RefProcess proc = Process::getSystemProcess();
	thread = Thread::createThread(init, 0, "Display Init Thread", proc);
	thread->run();
}

extern "C"
void __finalize module_finalize(void) {
	vesa = RefProcess(0);
	
	Namespace::unregisterVar(hw::console::version);
	Namespace::unregisterVar(hw::display::current::width);
	Namespace::unregisterVar(hw::display::current::height);
	Namespace::unregisterVar(hw::display::current::depth);
	Namespace::unregisterVar(hw::display::controller::name);
	Namespace::unregisterVar(hw::display::controller::vendor);
	Namespace::unregisterVar(hw::display::controller::memory);
	
	if (readEDID) {
		Namespace::unregisterVar(hw::monitor::edid::version);
		Namespace::unregisterVar(hw::monitor::edid::revision);
		Namespace::unregisterVar(hw::monitor::name);
		Namespace::unregisterVar(hw::monitor::vendor);
		Namespace::unregisterVar(hw::monitor::width);
		Namespace::unregisterVar(hw::monitor::height);
		Namespace::unregisterVar(hw::monitor::dpms::activeoff);
		Namespace::unregisterVar(hw::monitor::dpms::suspend);
		Namespace::unregisterVar(hw::monitor::dpms::standby);
	}
}

#include "draw.h"

#define _CLASS (syscallParam(regs, 0))
#define _FUNC (syscallParam(regs, 1))
#define _ARG(n) (syscallParam(regs, (n)+2))

static bool syscallHandler(regs_t *regs) {
	bool handled = false;
	
	if (_CLASS == SYSCALL_DISPLAY) {
		switch(_FUNC) {
			case SYSCALL_DISPLAY_QUERY:
				OUT("Display driver present query\n");
				regs->eax = (modeSet && _ARG(0) == 0xdeadbeef)
					? 0xfeedbeef : 0;
				break;
			
			case SYSCALL_DISPLAY_GET_MODEINFO: {
				display_modeinfo_t *mi = (display_modeinfo_t *) _ARG(0);
				mi->width = modeinfo.xResolution;
				mi->height = modeinfo.yResolution;
				mi->bpp = modeinfo.bitsPerPixel;
				mi->scanWidth = modeinfo.bytesPerScanLine;
			}
			break;
			
			case SYSCALL_DISPLAY_DRAW_STRING: {
				if (modeSet) {
					char str[200];
					strncpy(str, (char *) _ARG(0), sizeof(str));
					sint16 x = (sint16) _ARG(1);
					sint16 y = (sint16) _ARG(2);
					uint32 fg = _ARG(3);
					uint32 bg = _ARG(4);
					
					RefProcess current = Process::getCurrentProcess();
					RefProcess system = Process::getSystemProcess();
					system->getPageTable().makeCurrent();
					
					drawString(str, x, y, fg, bg);
					
					current->getPageTable().makeCurrent();
				}
			}
			break;
			
			case SYSCALL_DISPLAY_DRAW_LINE: {
				if (modeSet) {
					sint16 x0 = _ARG(0);
					sint16 y0 = _ARG(1);
					sint16 x1 = _ARG(2);
					sint16 y1 = _ARG(3);
					uint32 fg = _ARG(4);
					
					RefProcess current = Process::getCurrentProcess();
					RefProcess system = Process::getSystemProcess();
					system->getPageTable().makeCurrent();
					
					line(x0, y0, x1, y1, fg);
					
					current->getPageTable().makeCurrent();
				}
			}
			break;
			
			case SYSCALL_DISPLAY_DRAW_GRADIENT_LINE: {
				if (modeSet) {
					sint16 x0 = _ARG(0);
					sint16 y0 = _ARG(1);
					sint16 x1 = _ARG(2);
					sint16 y1 = _ARG(3);
					uint32 fg = _ARG(4);
					uint32 bg = _ARG(5);
					
					RefProcess current = Process::getCurrentProcess();
					RefProcess system = Process::getSystemProcess();
					system->getPageTable().makeCurrent();
					
					gradientLine(x0, y0, x1, y1, fg, bg);
					
					current->getPageTable().makeCurrent();
				}
			}
			break;
			
			case SYSCALL_DISPLAY_SOLID_FILL: {
				if (modeSet) {
					uint32 fg = _ARG(0);
					sint16 x0 = _ARG(1);
					sint16 y0 = _ARG(2);
					sint16 x1 = _ARG(3);
					sint16 y1 = _ARG(4);
					
					RefProcess current = Process::getCurrentProcess();
					RefProcess system = Process::getSystemProcess();
					system->getPageTable().makeCurrent();
					
					solidFill(fg, x0, y0, x1, y1);
					
					current->getPageTable().makeCurrent();
				}
			}
			break;
			
			case SYSCALL_DISPLAY_MAP_FRAME_BUFFER: {
				if (modeSet) {
					RefProcess current = Process::getCurrentProcess();
						
					Region *region = current->getAddressSpace().create(
						modeinfo.physicalBase, modeinfo.physicalBase,
						modeinfo.bytesPerScanLine*modeinfo.yResolution*2,
						REGION_FLAGS_CONTIG | REGION_FLAGS_USER
						| REGION_FLAGS_WIRED, REGION_TYPE_FRAMEBUFFER);
					
					regs->eax = region->getVirtualBase();
				} else {
					regs->eax = 0;
				}
			}
			break;
			
			case SYSCALL_DISPLAY_SWAP_BUFFERS:
				if (pipe) {
					regs->eax = swapBuffers(pipe) == VBE_SUCCESS;
				} else {
					OUT("Failed to swap buffers because pipe is null\n");
					regs->eax = false;
				}
				break;
		}
		
		handled = true;
	}
	
	return handled;
}

static bool initVesaInfo(RefMessagePipe mq) {
	static vbe_message_t message;
	
	message.function = VBE_CONTROLLER_INFO;
	mq->send(&message, 1, &message, sizeof(vbe_message_t));
	
	memcpy(&controller, &message.info, sizeof(vbe_controller_info_t));
	
	OUT("Fetched VBE controller info:\n");
	
	OUT("VBE return code %x\n", message.function);
	
	if (*(uint32 *)controller.vbeSignature != *(uint32 *)"VESA") {
		OUT("VBE not supported\n");
		return false;
	}
	
	OUT("VBE signature %.4s\n", controller.vbeSignature);
	OUT("VBE version %04x\n", controller.vbeVersion);
	OUT("Total memory %dK\n", controller.totalMemory * 64);
	hw::display::controller::memory = controller.totalMemory * 64;
	
	uint32 caps = controller.capabilities;
	OUT("Capabilities:\n%s%s%s%s%s",
		caps & 0x01 ? "    DAC width switchable to 8bits ppc\n" :
			"    DAC is fixed width, 6bits ppc\n",
		caps & 0x02 ? "    Controller is not VGA compatible\n" :
			"    Controller is VGA compatible\n",
		caps & 0x04 ? "    Use blank bit in fn09h for large RAMDAC\n" :
			"    Normal RAMDAC operation\n",
		caps & 0x08 ? "    No hardware stereoscopic signaling supported\n" :
			"    Hardware stereoscopic signaling supported\n",
		caps & 0x10 ? "    Stereo signaling via ext VESA connector\n" :
			"    Stereo signaling via VESA EVC connector\n"
	);
	
	OUT("OEM string: %s\n", &controller.oemData[controller.oemStringIdx]);
	
	if (controller.vbeVersionMajor >= 0x02) {
		OUT("OEM software revision %04x\n", controller.oemSoftwareRev);
		
		OUT("OEM vendor name: %s\n",
			&controller.oemData[controller.oemVendorNameIdx]);
		hw::display::controller::vendor =
			&controller.oemData[controller.oemVendorNameIdx];
			
		OUT("OEM product name: %s\n",
			&controller.oemData[controller.oemProductNameIdx]);
		hw::display::controller::name =
			&controller.oemData[controller.oemProductNameIdx];
			
		OUT("OEM product revision %s\n",
			&controller.oemData[controller.oemProductRevIdx]);
		
		//Log::hexdump(LOG_MEDIUM, (uint8 *) controller.oemData, 256, true);
	}
	
	parseEDID(mq);
	
	return true;
}

static bool checksum(const void *data, int len) {
	const sint8 *d = (const sint8 *) data;
	sint8 sum = 0;
	
	for (int i=0; i < len; i++) {
		sum += d[i];
	}
	
	return sum == 0;
}	

#include "edid.h"
static void parseEDID(RefMessagePipe mp) {
	static vbe_message_t message;
	
	// check for DDC extension support and info
	message.function = VBE_DDC_EXT;
	message.ddcext.function = VBE_DDC_EXT_GET_CAPS;
	message.ddcext.controller = 0;
	message.ddcext.blockNum = 0;
	
	mp->send(&message, sizeof(vbe_ddc_ext_t), &message, sizeof(vbe_message_t));
	
	if (message.return_code == VBE_SUCCESS) {
		memcpy(&ddcCaps, &message.ddccaps, sizeof(vbe_ddc_caps_t));
		
		message.function = VBE_DDC_EXT;
		message.ddcext.function = VBE_DDC_EXT_GET_EDID;
		message.ddcext.controller = 0;
		message.ddcext.blockNum = 0;
		
		mp->send(&message, sizeof(vbe_ddc_ext_t), &message,
			sizeof(vbe_message_t));
			
		if (message.return_code == VBE_SUCCESS
			&& checksum(&message.edid, sizeof(vbe_edid_t))) {
			memcpy(&edid, &message.edid, sizeof(vbe_edid_t));
		} else {
			// couldn't read EDID
			OUT("couldn't read EDID\n");
			return;
		}
	} else {
		// VBE/DCC not supported
		OUT("VBE/DCC not supported\n");
		return;
	}
	
	// verify EDID header
	if (strncmp((char *)edid.header, (char *)edid_v1_header,
		sizeof(edid.header))) {
		// header bytes didn't match
		return;
	}
	
	OUT("Read EDID\n");
	readEDID = true;
	
	hw::monitor::edid::version = edid.version;
	hw::monitor::edid::revision = edid.revision;
	
	// parse the vendor name
	static char sign[4];
	unsigned short h;
	
	/*
	 08h	WORD	big-endian manufacturer ID (see #00136)
		    bits 14-10: first letter (01h='A', 02h='B', etc.)
		    bits 9-5: second letter
		    bits 4-0: third letter
	*/
	h = COMBINE_HI_8LO(edid.manufacturerName[0], edid.manufacturerName[1]);
	sign[0] = ((h>>10) & 0x1f) + 'A' - 1;
	sign[1] = ((h>>5) & 0x1f) + 'A' - 1;
	sign[2] = (h & 0x1f) + 'A' - 1;
	sign[3] = 0;
	
	hw::monitor::vendor = sign;
	
	// max resolution from timings
	int wmax = 0;
	int hmax = 0;
	
	// monitor name from detailed timing description block
	uint8 *block = edid.detailedTimingDescription;
	for (int i=0; i < 4; i++, block += DETAILED_TIMING_DESCRIPTION_SIZE) {
		int blockType = UNKNOWN_DESCRIPTOR;
		
		if (!strncmp( (char *) edid_v1_descriptor_flag, (char *) block, 2 )) {
			if (block[2] == 0) {
				blockType = block[3];
			}
		} else {
			blockType = DETAILED_TIMING_BLOCK;
		}
		
		switch (blockType) {
			case MONITOR_NAME: {
				char name[13];
				uint8 *ptr = block + DESCRIPTOR_DATA;
				
				for (int j=0; j < 13; j++) {
					if (ptr[j] == 0x0a) {
						name[j] = '\0';
						break;
					} else {
						name[j] = ptr[j];
					}
				}
				
				hw::monitor::name = name;
			}
			break;
			
			case DETAILED_TIMING_BLOCK: {
				byte *dtd = block;
				
				int w = H_ACTIVE;
				int h = V_ACTIVE;
				
				if ( (w*w + h*h) > (wmax*wmax + hmax*hmax) ) {
					wmax = w;
					hmax = h;
				}
			}
			break;
		}
	}
	
	for (int i=0; i < 8; i++) {
		uint16 mode = edid.standardTimingIdentification[i];
		int w = (mode & 0xff) * 8 + 248;
		int aspect = mode >> 14;
		int h = 0;
		
		switch (aspect) {
			case 0: // 16:10
				h = w*10/16;
				break;
			
			case 1: // 4:3
				h = w*3/4;
				break;
			
			case 2: // 5:4
				h = w*4/5;
				break;
			
			case 3: // 16:9
				h = w*9/16;
				break;
		}
		
		if ( (w*w + h*h) > (wmax*wmax + hmax*hmax) ) {
			wmax = w;
			hmax = h;
		}
	}
		
	hw::monitor::width = wmax;
	hw::monitor::height = hmax;
		
	OUT("EDID display max: %ux%u\n", wmax, hmax);
	
	hw::monitor::dpms::activeoff = (edid.featureSupport & DPMS_ACTIVE_OFF) != 0;
	hw::monitor::dpms::suspend = (edid.featureSupport & DPMS_SUSPEND) != 0;
	hw::monitor::dpms::standby = (edid.featureSupport & DPMS_STANDBY) != 0;
}

static bool setMode(RefMessagePipe mp, uint32 width, uint32 height,
	uint8 bitsPerPixel) {
	static vbe_message_t message;
	uint32 i = controller.videoModeIdx;
	uint32 count = 0;
	uint32 linearCount = 0;
	int mode = -1;
	
	const uint16 modeMask = VBE_MODE_ATTR_HW_SUPPORT | VBE_MODE_ATTR_LINEAR_FB
		| VBE_MODE_ATTR_GRAPHIC;
	
	while (controller.modes[i] != 0xffff) {
		OUT("%02d. %04d\n", count, controller.modes[i]);
		
		// get information about the mode
		message.function = VBE_MODE_INFO;
		message.mode = controller.modes[i];
		mp->send(&message, 3, &message, sizeof(vbe_message_t));
		
		if (message.modeinfo.modeAttributes & modeMask == modeMask) {
			OUT("    %dx%d %dbpp, mem model: %02x, max images: %d\n",
				message.modeinfo.xResolution, message.modeinfo.yResolution,
				message.modeinfo.bitsPerPixel, message.modeinfo.memoryModel,
				message.modeinfo.numImagePages);
			
			if (mode == -1 && message.modeinfo.xResolution == width
				&& message.modeinfo.yResolution == height
				&& message.modeinfo.bitsPerPixel == bitsPerPixel) {
				mode = controller.modes[i];
				memcpy(&modeinfo, &message.modeinfo, sizeof(vbe_modeinfo_t));
				modeSet = true;
				
				OUT("Found desired mode!\n");
			}
			
			if (controller.vbeVersion == 0x0300
				&& message.modeinfo.xResolution == width
				&& message.modeinfo.yResolution == height
				&& message.modeinfo.bitsPerPixel >= 24) {
				OUT("%dx%d bpp=%d pos (%d, %d, %d, %d)\n",
					message.modeinfo.xResolution, message.modeinfo.yResolution,
					message.modeinfo.bitsPerPixel,
					message.modeinfo.linearRedMaskSize,
					message.modeinfo.linearGreenMaskSize,
					message.modeinfo.linearBlueMaskSize,
					message.modeinfo.linearRsvdMaskSize
				);
				
				//SLEEP(2000);
			}
			
			linearCount++;
		}	
		
		i++;
		count++;
	}
	OUT("Video modes: %d (%d linear graphical)\n", count, linearCount);
	
	if (mode != -1) {
		message.function = VBE_SET_MODE;
		message.setmode.mode = mode;
		message.setmode.useCRTC = 0;
		message.setmode.linear = 1;
		message.setmode.noClear = 0;
		mp->send(&message, 3, &message, 1);
		
		if (message.return_code != 0) {
			OUT("Mode set failed with code %02x\n", message.return_code);
			return false;
		}
		
		if (modeinfo.physicalBase == 0) {
			OUT("Could not obtain physical address of framebuffer\n");
			return false;
		}
		
		hw::display::current::width = modeinfo.xResolution;
		hw::display::current::height = modeinfo.yResolution;
		hw::display::current::depth = modeinfo.bitsPerPixel;
		
		RefProcess proc = Process::getSystemProcess();
		Region *region = proc->getAddressSpace().create(modeinfo.physicalBase,
			modeinfo.physicalBase,
			modeinfo.bytesPerScanLine*modeinfo.yResolution*2,
			REGION_FLAGS_CONTIG | REGION_FLAGS_WIRED, REGION_TYPE_FRAMEBUFFER);
		
		frameBuffer = (uint8 *) region->getVirtualBase();
			
		OUT("Framebuffer at %08x\n", modeinfo.physicalBase);
		frameBufferOffset = 0;
		frameBufferSize = modeinfo.bytesPerScanLine*modeinfo.yResolution;
		
		// init the framebuffer to clear out any possible junk in it
		solidFill(0x00336699);
	} else {
		modeSet = false;
		return false;
	}
	
	OUT("Done\n");
	return true;
}

static int swapBuffers(RefMessagePipe mp) {
	vbe_message_t message;
	
	frameBufferOffset = frameBufferOffset == 0 ? frameBufferSize : 0;
	
	message.function = VBE_DISPLAY_START;
	message.setstart.function = VBE_DISP_START_SET_AT_VRETRACE2;
	message.setstart.displayStart = frameBufferOffset;
	mp->send(&message, sizeof(vbe_set_display_start_t), &message,
		sizeof(vbe_set_display_start_t));
	
	return message.return_code;
}

static void drawString(const char *str, int x, int y, uint32 fg, uint32 bg) {
	// FIXME: we need generalized font support bad, use 8x12 bitmap for now
	texinfo tex = { 0, 0, 8, 12, 0 };
	
	for (int i=0; str[i] != '\0'; i++) {
		tex.mem = (uint8 *) &terminal_font[str[i]*12];
		drawBitmap(&tex, x + i*8, y, fg, bg);
	}
}

static void solidFill(uint32 color) {
	solidFill(color, 0, 0, modeinfo.xResolution, modeinfo.yResolution);
}

static void solidFill(uint32 color, sint16 xs, sint16 ys, uint16 w, uint16 h) {
	uint32 *fb = (uint32 *) (frameBuffer + frameBufferOffset);
    
    // clip left and top
    uint16 xo = xs < 0 ? 0 : xs;
    uint16 yo = ys < 0 ? 0 : ys;
    
	for (uint16 y=yo; y < ys + h && y < modeinfo.yResolution; y++) {
		for (uint16 x=xo; x < xs + w && x < modeinfo.xResolution; x++) {
				
			uint32 i = ((modeinfo.bytesPerScanLine * y)/4 + x);
			fb[i] = color;
		}
	}		
}

static void drawBitmap(const texinfo *tex, sint16 x, sint16 y, uint32 fg,
	uint32 bg) {
	if (tex->mem != 0 && tex->bpp == 0) {	  
		uint32 *fb = (uint32 *) (frameBuffer + frameBufferOffset);
		
		// clip left and top
	    uint16 xo = x < 0 ? 0 : x;
	    uint16 yo = y < 0 ? 0 : y;
	    
		for (uint16 j=0; j < tex->height
			&& (j + yo) < modeinfo.yResolution; j++) {
				
			for (uint16 i=0; i < tex->width
				&& (i + xo) < modeinfo.xResolution; i++) {
					
				uint32 b = (tex->width * j + i) >> 3;
				uint8 bit = (tex->width * j + i) & 0x7;
				uint8 mask = 1 << (7 - bit);
				
				if (tex->mem[b] & mask) {
					fb[(modeinfo.bytesPerScanLine * (j + yo))/4 + (i + xo)] = fg;
				} else if (bg != ~0U) {
					fb[(modeinfo.bytesPerScanLine * (j + yo))/4 + (i + xo)] = bg;
				}
			}
		}
	}
}

static void fillBitmap(const texinfo *tex, sint16 xs, sint16 ys, uint16 w,
	uint16 h, uint32 fg, uint32 bg) {
    if (tex->mem != 0 && tex->bpp == 0) {
    	// clip left and top
	    uint16 xo = xs < 0 ? 0 : xs;
	    uint16 yo = ys < 0 ? 0 : ys;
	    
	    uint32 *fb = (uint32 *) (frameBuffer + frameBufferOffset);
	    
	    for (uint16 y=yo; y < ys + h && y < modeinfo.yResolution; y++) {
			for (uint16 x=xo; x < xs + w && x < modeinfo.xResolution; x++) {
					
				uint32 fbIndex = (modeinfo.bytesPerScanLine * y)/4 + x;
				uint32 bmIndex = (tex->width * (y % tex->height)
					+ (x % tex->width));
				
				uint32 b = (bmIndex) >> 3;
				uint8 bit = (bmIndex) & 0x7;
				uint8 mask = 1 << (7 - bit);
				
				if (tex->mem[b] & mask) {
					fb[fbIndex] = fg;
				} else if (bg != ~0U) {
					fb[fbIndex] = bg;
				}
			}
		}
    }
} 
